#include <stdbool.h>
#include <string.h>
#include "dlt645_protocol.h"

const static int min_len_requests = sizeof(dlt645_head_t) + sizeof(dlt645_tail_t);

dlt645_t *decode_dlt645(const uint8_t *buf, size_t size, dlt645_t *pkg, size_t *nums_read) {
    dlt645_t *ret = NULL;
    const uint8_t *ps = buf, *pe = buf + size;
    uint8_t check_sum = 0;

    if (NULL == pkg)
        return NULL;

    while (ps < pe && PRE_DLT645 == *ps) {
        ++ps;
    }

    while (ps < pe && BOF_DLT645 != *ps) {
        ++ps;
    }

    if ((pe - ps) >= min_len_requests) {
        int i;
        const uint8_t *cur = ps, *cs = ps;

        memmove(&pkg->head, cur, sizeof(pkg->head));
        cur += sizeof(pkg->head);

        if ((pe - cur) >= pkg->head.data_len + sizeof(pkg->tail)) {
            memmove(pkg->data, cur, pkg->head.data_len);
            cur += pkg->head.data_len;

            for (i = 0; i < pkg->head.data_len; ++i) {
                pkg->data[i] -= DEL_DLT645;
            }

            while (cs < cur) {
                check_sum += *cs;
                ++cs;
            }

            memmove(&pkg->tail, cur, sizeof(pkg->tail));
            cur += sizeof(pkg->tail);

            pe = cur;

            if (BOF_DLT645 != pkg->head.bof0 || BOF_DLT645 != pkg->head.bof1) {
                return NULL;
            }

            if (EOF_DLT645 != pkg->tail.eof || check_sum != pkg->tail.cs) {
                return NULL;
            }
            ret = pkg;
        }
    }

    if (NULL != nums_read) {
        if (NULL != ret) {
            *nums_read = (pe - buf);
        } else {
            *nums_read = (ps - buf);
        }
    }

    return ret;
}

int encode_dlt645(uint8_t *buf, size_t size, dlt645_t *pkg) {
    uint8_t *cur = buf, *cs = buf;

    if (size < min_len_requests) {
        return -1;
    }

    if (NULL == pkg) {
        return -1;
    }

    if (size >= sizeof(pkg->head)) {
        pkg->head.bof0 = BOF_DLT645;
        pkg->head.bof1 = BOF_DLT645;

        memmove(cur, &pkg->head, sizeof(pkg->head));
        cur += sizeof(pkg->head);
        size -= sizeof(pkg->head);
    } else {
        return -1;
    }

    if (size >= pkg->head.data_len) {
        int i = 0;
        for (; i < pkg->head.data_len; ++i) {
            cur[i] = pkg->data[i] + DEL_DLT645;
        }
        cur += pkg->head.data_len;
        size -= pkg->head.data_len;
    } else {
        return -1;
    }

    if (size >= sizeof(pkg->tail)) {
        pkg->tail.cs = 0;
        while (cs < cur) {
            pkg->tail.cs += *cs;
            ++cs;
        }
        pkg->tail.eof = EOF_DLT645;

        memmove(cur, &pkg->tail, sizeof(pkg->tail));
        cur += sizeof(pkg->tail);
    } else {
        return -1;
    }

    return cur - buf;
}

static _Bool _init_dlt645(dlt645_t *pkg, const uint8_t *addr) {
    if (NULL == pkg)
        return false;

    memmove(pkg->head.addr, addr, sizeof(pkg->head.addr));
    pkg->head.ctrl_code.direction = 0;
    pkg->head.ctrl_code.exception = 0;
    pkg->head.ctrl_code.follow = 0;
    pkg->head.ctrl_code.code = CODE_RESERVE;

    pkg->head.data_len = 0;

    return true;
}

_Bool create_read_data_req(dlt645_t *pkg, const uint8_t *addr, protocol_e protocol, uint32_t di) {
    if (!_init_dlt645(pkg, addr))
        return false;

    switch (protocol) {
        case cp_dlt645_97:
            pkg->head.ctrl_code.code = V97_READ_DATA;
            pkg->head.data_len = 2;
            break;
        case cp_dlt645_07:
            pkg->head.ctrl_code.code = V07_READ_DATA;
            pkg->head.data_len = 4;
            break;
        default:
            return false;
    }
    memmove(pkg->data, &di, pkg->head.data_len);

    return true;
}

_Bool create_write_data_req(dlt645_t *pkg, const uint8_t *addr, protocol_e protocol, uint32_t di,
                            const uint8_t *data, size_t size) {
    if (size > DLT645_WRITE_PKG_SIZE)
        return false;

    if (!_init_dlt645(pkg, addr))
        return false;

    switch (protocol) {
        case cp_dlt645_97:
            pkg->head.ctrl_code.code = V97_WRITE_DATA;
            pkg->head.data_len = 2 + size;
            memmove(pkg->data, &di, 2);
            memmove(&pkg->data[2], data, size);
            break;
        case cp_dlt645_07:
            pkg->head.ctrl_code.code = V07_WRITE_DATA;
            pkg->head.data_len = 4 + size;
            memmove(pkg->data, &di, 4);
            memmove(&pkg->data[4], data, size);
            break;
        default:
            return false;
    }

    return true;
}
